// 测试库：SimpleFOC 2.2.1
//测试硬件：SimpleFOC_X V1.0驱动板
//淘宝店铺：YOUNGFOC    B站：LazierWoo     闲鱼店铺：YOUNGFOC  立创开源广场 用户名：LazyWoo
//程序功能：力矩闭环控制，本质就是电流环闭环。在串口窗口中输入：T+电流。
//比如让电机相电流达到0.1A，则输入其弧度制：T0.1

//☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆
//注意事项：输入的目标电流值-0.5A到0.5A，请勿长时间将电流设置在0.4A以上，请勿长时间堵转
//☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆

//☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆
//注意事项：请将单次电流变化设置在0.4A以内，以免电机失控，高速运转。
//☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆


//☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆
//☆☆☆☆☆☆程序默认设置的供电电压为 12V,用其他电压供电请记得修改!!!!!☆☆☆☆☆☆☆☆☆
//☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆


#include <SimpleFOC.h>

//创建电机对象
BLDCMotor motor = BLDCMotor(7); //极对数
//创建驱动器对象
//3PWM模式，定义三个PWM输出口，以及使能口
BLDCDriver3PWM driver = BLDCDriver3PWM(25, 32, 33,26);


//创建AS5600磁编码器对象
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
TwoWire I2Cone = TwoWire(0);

//创建内置电流传感器，两个ADC引脚对应两相的电流采样
//采样电阻10mΩ，0.01欧姆，INA240A2放大倍数50.
InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, 34, 35);

/*
 * 
 * BLDCDriver3PWM driver = BLDCDriver3PWM(25, 32, 33,26);
 * InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, 34, 35);
 * 这两句中，25与34，32与35是相互对应的电机相，顺序不要修改
*/


//命令设置
//设置目标力矩
float target_torque = 0;
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&target_torque, cmd); }


void setup() 
{

  //磁编码器初始化
  I2Cone.begin(19,18, 400000UL);
  sensor.init(&I2Cone);
  //关联电机对象与磁编码器对象
  motor.linkSensor(&sensor);

  // 驱动器参数
  // ☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆供电电压☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆
  driver.voltage_power_supply = 24.0;  //板子上带有防反接二极管，在使用过程中约有0.3V的压降，可以减去0.3，也可以忽略
  //驱动器初始化
  driver.init();
  //关联电机对象和驱动器对象
  motor.linkDriver(&driver);

  // 电流传感器初始化
  current_sense.init();
  //关联电机对象和电流传感器对象
  motor.linkCurrentSense(&current_sense);
  
  //力矩控制模式选择：FOC电流（foc_current）、DC电流（dc_current）、电压（voltage）
  motor.torque_controller = TorqueControlType::foc_current; 
  // 运动控制模式选择：力矩（torque）、速度(velocity)、位置(angle)
  motor.controller = MotionControlType::torque;

  //FOC电流环参数，只有在电流闭环时才有用
  //dc_current模式下只有Current q项PID
  //foc_current模式下Current q项PID、Current d项PID都起作用

  //Current q项参数
   motor.PID_current_q.P = 3.0;
   motor.PID_current_q.I = 500.0;
   motor.PID_current_q.D = 0.0;
   motor.PID_current_q.output_ramp = 100.0;
   //motor.PID_current_q.limit = 6.0;  //电流环的输出限制，应该等于限制电压motor.voltage_limit；
   //低通滤波器 
   motor.LPF_current_q.Tf = 0.005;

  //Current d项参数
  motor.PID_current_d.P = 3.0;
  motor.PID_current_d.I = 500.0;
  motor.PID_current_d.D = 0.0;
  motor.PID_current_d.output_ramp = 100.0;
  //motor.PID_current_d.limit = 6.0;  //电流环的输出限制，应该等于限制电压motor.voltage_limit；
  //低通滤波器 
  motor.LPF_current_d.Tf = 0.005;

   
  // 速度环PID参数
  motor.PID_velocity.P = 0.12;
  motor.PID_velocity.I = 1.2;
  motor.PID_velocity.D = 0.001;
  motor.PID_velocity.output_ramp = 1000.0;
  //速度环的输出限制:在电流闭环时，此限制应该等于限制电流motor.current_limit；
  //速度环的输出限制:在使用电压力矩时（电流开环），此限制应该等于限制电压motor.voltage_limit； 
  //motor.PID_velocity.limit = 0.8; 
  // 低通滤波器
  motor.LPF_velocity.Tf = 0.02;
  
  // 角度环PID参数
  motor.P_angle.P = 10.0;
  motor.P_angle.I = 0.0;
  motor.P_angle.D = 0.0;
  motor.P_angle.output_ramp = 1000.0;
  //motor.P_angle.limit = 30.0;    //角度环的输出限制就等于速度限制velocity_limit
  // 低通滤波器
  motor.LPF_angle.Tf = 0.00;

  
  // 设置限制
  motor.velocity_limit = 30.0; //速度限制，指角度模式下的速度限制
  //把速度限制设小，比如设置成3.0或者5.0，角度控制模式下，最高速度会变小，
  motor.voltage_limit = 6.0;  //电机限制电压
  motor.current_limit = 0.6;  //电机限制电流


  //串口用于接受指令
  Serial.begin(115200);
  motor.useMonitoring(Serial);


  // 电机初始化
  motor.init();
  // 初始化FOC
  motor.initFOC();

  //使用字母T作为命令前缀
  command.add('T', doTarget, "target torque");

  //初始化成功，等待接收命令
  Serial.println(F("Motor ready."));
  Serial.println(F("Set the target torque using serial terminal:"));
  delay(1000);
}

void loop() 
{
  //FOC运行
  motor.loopFOC();
  motor.move(target_torque);
  command.run();
}
